/**
  ******************************************************************************
  * @file    can_open.c
  * @author  Dajin Li
  * @version V0.0.1
  * @date    08-08-2015 
  * @brief   This file contains CANopen specific functions.  
  *          
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; KION(SZ) R&D Center
  *
  * Licensed under KION, (the "License");
  *
  ******************************************************************************
  */ 

/* Includes ------------------------------------------------------------------*/
#include <string.h>
#include "stm32f10x.h"
#include "stm32f10x_can.h"
#include "can.h"
#include "can_open.h"
#include "var_tbl.h"
#include "global.h"
#include "task.h"
#include "lcd.h"
#include "Instrument.h"

#include "spi.h"

/********************************************************************************************************
*Local variables
********************************************************************************************************/
//static u16 heartbeat_identifier = 0;
/********************************************************************************************************
*Global variables
********************************************************************************************************/
s16 canopen_heart_beat_rate = 0;
s16 canopen_emergency_rate = 0;

s16 can_pdo_timeout_period = 3000;

s16 canopen_system_status_flags = 0;
s16 can_nmt_state = 0;
// Working Periodic Service Rates
s16 canopen_heart_beat_timer = 0;
s16 canopen_emergency_timer = 0;
s16 can_pdo_timeout = 0;
union CAN_SYSTEM_FLAGS CanSystemFlags = {0};
s16 sbu_state = SBU_IDLE;
s16 sbu_sequence_number = 0;
//static unsigned int xhuge *initial_sdo_upload_flash_ptr;
u8 sdo_data[8];
u8 sdo_tx_data[8];
static s16 kwp_data_index = 0;
static u16 kwp_rx_data_length = 0;
static s16 kwp_data_length = 0;
static s16 kwp_var_index;
static s16 kwp_object_id;
s8 kwp_needs_ee;
u8 kwp_ee_index;
u8 secacc_security_state;
u8 cal_cmd;
u8 cal_response;
u8 last_seq_num = 0;

u16 blkIndex = 0;

CanTxMsg d_txMsg[E_TX_BUFF_SIZE];
u8	d_tx_index = 0;

u8 start_programing = 0;
u8 sdo_dlc;
u8 Can_Rx_buf[BUFFER_SIZE];
u16 dataIndex = 0;

s32 flashing = 0;
static u32 secacc_code;
static u8 secacc_expected_service;
static enum secacc_state secacc_security_state;

u32 startAddr;
u32 endAddr;
u32 flash_length;
u16 checksum;
u16 temp_checksum;
u32 recevie_count;
static u8  *multi_char_ptr = 0;
u8 multi_send;

struct fortune fortune_fate;

/********************************************************************************************************
*Local functions
********************************************************************************************************/
static int setup_kwp(void);
static void process_kwp_mosi(CanRxMsg* RxMessage);
static u32 secacc_crypto_12( u32 seed );
static u32 secacc_crypto_34( u32 seed);
static u32 secacc_crypto_56( u32 seed);
enum kwp_response_code secacc_process_request( u8 condition, u8  *dat, u16 *len);
enum secacc_state secacc_get_state( void );
static u32 secacc_fortuna( void );
static void send_kwp_init01_response(void);


/**
* @brief	canopen_init
* @param	none
* @note     none
* @retval   int
*/
int canopen_init(void)
{
    u8 result = 1;

    result = setup_kwp();
    if(result == 0) 
    {
        return(0);
    }
    
    can_nmt_state = NMT_INITIALIZATION;
    canopen_system_status_flags = canopen_system_status_flags | CAN_OPEN_ENABLED;
    
    return(1);
}

/**
* @brief	setup for kwp2000
* @param	none
* @note     none
* @retval   int
*/
static int setup_kwp(void)
{
    u16 temp_cob_id = 0;
    CAN_FilterInitTypeDef  CAN_FilterInitStructure = {0};
    
    temp_cob_id = DISP_KWP_RX_ID;
    
    /* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_0;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CANOPEN_CAN_FIFO;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);


    temp_cob_id = DISP_KWP_RX_ID_ALL;
    
    /* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_1;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CANOPEN_CAN_FIFO;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);

    /*##-2- Configure the CAN Filter ###########################################*/
    temp_cob_id = DISP_KWP_INIT01_RX_ID_ALL;
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_2;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CANOPEN_CAN_FIFO;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);
    

    return(1);
}


/**
* @brief	send sdo reponse
* @param	s16 type: reponse type
* @param	u32 result: reponse type
* @note     none
* @retval   s16
*/
void send_kwp_response(void)
{
    CanTxMsg txMsg = {0};
    u32 cob_id = 0;

    cob_id = DISP_KWP_TX_ID;

    txMsg.Data[0] = sdo_data[0];
    txMsg.Data[1] = sdo_data[1];
    txMsg.Data[2] = sdo_data[2];
    txMsg.Data[3] = sdo_data[3];
    txMsg.Data[4] = sdo_data[4];
    txMsg.Data[5] = sdo_data[5];
    txMsg.Data[6] = sdo_data[6];
    txMsg.Data[7] = sdo_data[7];

    txMsg.IDE = CAN_STD_ID;
    txMsg.StdId = cob_id; 
    txMsg.DLC = CAN_DATA_LEN_DEF;

    CAN_Transmit(CANOPEN_CAN, &txMsg);
    return;
}
/**
* @brief	set nmt state
* @param	s16 command_specifier
* @note     none
* @retval   none
*/
s16 set_nmt_state(s16 command_specifier)
{
    switch(command_specifier) 
    {
    case NMT_CS_RESET_NODE:
        can_nmt_state = NMT_INITIALIZATION;
        CAN_PDO_Enabled = 0;
        CAN_PDO_Timeouts_On = 0;
        can_pdo_timeout = can_pdo_timeout_period;
        CAN_SDO_Enabled = 0;
        PDO_Timed_Out = 0;
        CAN_Enabled = 0;
	    //reset_controller();
        break;

    case NMT_CS_RESET_COMMUNICATION:
        can_nmt_state = NMT_PRE_OPERATIONAL;
        CAN_PDO_Enabled = 0;
        CAN_PDO_Timeouts_On = 0;
        can_pdo_timeout = can_pdo_timeout_period;
        CAN_SDO_Enabled = 1;
        PDO_Timed_Out = 0;
        CAN_Config();
        break;

    case NMT_CS_STOP_REMOTE_NODE:
        can_nmt_state = NMT_STOPPED;
        CAN_PDO_Enabled = 0;
        CAN_PDO_Timeouts_On = 0;
        can_pdo_timeout = can_pdo_timeout_period;
        CAN_SDO_Enabled = 0;
        PDO_Timed_Out = 0;
        break;

    case NMT_CS_ENTER_PRE_OPERATIONAL:
        can_nmt_state = NMT_PRE_OPERATIONAL;
        CAN_PDO_Enabled = 0;
        CAN_PDO_Timeouts_On = 0;
        can_pdo_timeout = can_pdo_timeout_period;
        CAN_SDO_Enabled = 1;
        break;

    case NMT_CS_START_REMOTE_NODE:
        can_nmt_state = NMT_OPERATIONAL;
        CAN_PDO_Enabled = 1;
        CAN_SDO_Enabled = 1;
        PDO_Timed_Out = 0;
    	CAN_Time_Out = 0;
    	break;

    default:
        return 0;
    }
    return 1;
}


/**
* @brief	process kwp message
* @param	CanRxMsg* RxMessage: buffer
* @note     none
* @retval   none
*/
static void process_kwp_mosi(CanRxMsg* RxMessage)
{
    u8 service_id;
    u32 *long_ptr = 0;
    u16 *short_ptr = 0;  
    static u8  *char_ptr = 0; 
    u8 frame_type = 0;
    u32 i, k;
    u32 temp_data;
    u8 entry_option;
    u32 min_value;
    u32 max_value;
    u16 attr;
    FLASH_Status status = FLASH_COMPLETE;
    u16 data_length;
    enum kwp_response_code rv;
//   u16 ee_addr;

    u8 rx_completed = 0;

    if(RxMessage == NULL )
    {
        return;
    }


    sdo_data[0] = RxMessage->Data[0];
    sdo_data[1] = RxMessage->Data[1];
    sdo_data[2] = RxMessage->Data[2];
    sdo_data[3] = RxMessage->Data[3];
    sdo_data[4] = RxMessage->Data[4];
    sdo_data[5] = RxMessage->Data[5];
    sdo_data[6] = RxMessage->Data[6];
    sdo_data[7] = RxMessage->Data[7];

    sdo_dlc = RxMessage->DLC;
    

   
    frame_type = (sdo_data[0] & 0xF0) >> 4;


    if(frame_type == KWP_SINGLE_FRM)
    {
        dataIndex = 0;
        for(i = 1; i < sdo_dlc; i++)
        {
            Can_Rx_buf[dataIndex++] = sdo_data[i];
        }
        rx_completed = 1;
        kwp_rx_data_length = sdo_data[0] & 0x0F;
        recevie_count = 0;
        multi_send = 0;
    }
    else if(frame_type == KWP_FIRST_FRM)
    {
        dataIndex = 0;
        for(i = 2; i < sdo_dlc; i++)
        {
            Can_Rx_buf[dataIndex++] = sdo_data[i];
        }
        rx_completed = 0;
        kwp_rx_data_length = ((((s16)sdo_data[0]) & 0x0F) << 8) | sdo_data[1];
        blkIndex = 1;
        sdo_data[0] = 0x30;
        sdo_data[1] = 0;   // default session
        sdo_data[2] = 0x00;   // default session
        sdo_data[3] = 0;
        sdo_data[4] = 0;
        sdo_data[5] = 0;
        sdo_data[6] = 0;
        sdo_data[7] = 0;

        send_kwp_response();
    }
    else if(frame_type == CONSECUTIVE_FRM)
    {
        i = sdo_data[0] & BLK_INDEX_MASK; // Get block index
        if(i == blkIndex)
        {
            for(i = 1; i < sdo_dlc; i++)
            {
                Can_Rx_buf[dataIndex++] = sdo_data[i];
            }
            if(dataIndex >= kwp_rx_data_length)
            {
                rx_completed = 1;
            }
        }
        blkIndex++;
        blkIndex %= 16;
    }
    else if(frame_type == KWP_FLOW_CONTROL_FRM)
    {
        multi_char_ptr = char_ptr;
        multi_send = 1;
        blkIndex = 1;
    }



    if(rx_completed == 1)
    {
        dataIndex = 0;
        service_id = Can_Rx_buf[dataIndex++];
        switch(service_id)
        {
        case KWP_START_DEFAULT_SESSION:
            kwp_object_id = Can_Rx_buf[dataIndex++];
            if(kwp_object_id == KWP_DEFAULT_SESSION)
            {
                secacc_security_state = secacc_state_default;                                      // default session
                sdo_data[0] = 2;
                sdo_data[1] = KWP_START_DEFAULT_SESSION_OK;
                sdo_data[2] = KWP_DEFAULT_SESSION;   // default session
                sdo_data[3] = 0;
            }
            else if(kwp_object_id == KWP_START_PROGRAMING_SESSION)
            {
                if (secacc_get_state() != secacc_state_programming)
                {
                    sdo_data[0] = 3;
                    sdo_data[1] = KWP_FAIL;
                    sdo_data[2] = service_id;                       // default session
                    sdo_data[3] = KWP_RESPONSE_CODE_ACCESSDENIED;   // default session
                }
                else
                {
                    sdo_data[0] = 2;
                    sdo_data[1] = KWP_START_DEFAULT_SESSION_OK;
                    sdo_data[2] = kwp_object_id;   // default session
                    sdo_data[3] = 0;
                }
            }
            sdo_data[4] = 0;
            sdo_data[5] = 0;
            sdo_data[6] = 0;
            sdo_data[7] = 0;
            start_programing = 0;
            dataIndex = 0;
            send_kwp_response();
            start_programing = 0;
            break;
        
        case KWP_RD_ECU:
            kwp_object_id = Can_Rx_buf[dataIndex++];
            kwp_var_index = lookup_object_id(kwp_ecu_params, MAX_KWP_ECU_OBJECT_DEF, kwp_object_id);
            if (kwp_var_index < 0)
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = service_id;
                sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            }
            else
            {
                attr = kwp_ecu_params[kwp_var_index].attr;
                switch(attr & VAR_LENGTH_MASK) 
                {
                case FOUR_BYTES_LEN:
                    long_ptr = (u32 *) kwp_ecu_params[kwp_var_index].var;
                    sdo_data[0] = 5;
                    sdo_data[1] = RD_ECU_OK;
                    sdo_data[2] = (u8)((*long_ptr) & 0x000000FF);
                    sdo_data[3] = (u8)(((*long_ptr) & 0x0000FF00) >> 8);
                    sdo_data[4] = (u8)(((*long_ptr) & 0x00FF0000) >> 16);
                    sdo_data[5] = (u8)(((*long_ptr) & 0xFF000000) >> 24);
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                case TWO_BYTES_LEN:                          
                    short_ptr = (u16 *) kwp_ecu_params[kwp_var_index].var;
                    sdo_data[0] = 3;
                    sdo_data[1] = RD_ECU_OK;
                    sdo_data[2] = (u8)((*short_ptr) & 0x000000FF);
                    sdo_data[3] = (u8)(((*short_ptr) & 0x0000FF00) >> 8);
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                case ONE_BYTES_LEN:
                    char_ptr = (u8 *) kwp_ecu_params[kwp_var_index].var;
                    sdo_data[0] = 2;
                    sdo_data[1] = RD_ECU_OK;
                    sdo_data[2] = (u8)(*char_ptr);
                    sdo_data[3] = 0;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                default:
                    char_ptr = (u8 *) kwp_ecu_params[kwp_var_index].var;
                    kwp_data_length = strlen((char *)char_ptr);
                    kwp_data_length += 1;
                    kwp_data_index = 0;
                    sdo_data[0] = (1 << 4) | ((kwp_data_length >> 8) & 0x0F);
                    sdo_data[1] = kwp_data_length & 0xFF;
                    sdo_data[2] = RD_ECU_OK;
                    sdo_data[3] = char_ptr[kwp_data_index++];
                    sdo_data[4] = char_ptr[kwp_data_index++];
                    sdo_data[5] = char_ptr[kwp_data_index++];
                    sdo_data[6] = char_ptr[kwp_data_index++];
                    sdo_data[7] = char_ptr[kwp_data_index++];
                    send_kwp_response();
                    kwp_data_length -= 6;

                    break; 
                }
            }
            break;

        case KWP_RD_DATA:
            kwp_object_id = Can_Rx_buf[dataIndex++];
            kwp_var_index = lookup_object_id(kwp_data_params, MAX_KWP_DATA_OBJECT_DEF, kwp_object_id);
            if (kwp_var_index < 0)
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = service_id;
                sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            }
            else
            {
                switch(kwp_data_params[kwp_var_index].attr & VAR_LENGTH_MASK) 
                {
                case FOUR_BYTES_LEN:
                    long_ptr = (u32 *) kwp_data_params[kwp_var_index].var;
                    sdo_data[0] = 6;
                    sdo_data[1] = KWP_RD_DATA_OK;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = (u8)((*long_ptr) & 0x000000FF);
                    sdo_data[4] = (u8)(((*long_ptr) & 0x0000FF00) >> 8);
                    sdo_data[5] = (u8)(((*long_ptr) & 0x00FF0000) >> 16);
                    sdo_data[6] = (u8)(((*long_ptr) & 0xFF000000) >> 24);
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                case TWO_BYTES_LEN:   
                    short_ptr = (u16 *) kwp_data_params[kwp_var_index].var;
                    sdo_data[0] = 4;
                    sdo_data[1] = KWP_RD_DATA_OK;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = (u8)((*short_ptr) & 0x000000FF);
                    sdo_data[4] = (u8)(((*short_ptr) & 0x0000FF00) >> 8);
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                case ONE_BYTES_LEN:
                    char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                    sdo_data[0] = 3;
                    sdo_data[1] = KWP_RD_DATA_OK;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = (u8)(*char_ptr);
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                default:
                    char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                    kwp_data_length = strlen((char *)char_ptr);
                    kwp_data_length += 2;
                    kwp_data_index = 0;
                    sdo_data[0] = (1 << 4) | ((kwp_data_length >> 8) & 0x0F);
                    sdo_data[1] = kwp_data_length & 0xFF;
                    sdo_data[2] = KWP_RD_DATA_OK;
                    sdo_data[3] = kwp_object_id;
                    sdo_data[4] = char_ptr[kwp_data_index++];
                    sdo_data[5] = char_ptr[kwp_data_index++];
                    sdo_data[6] = char_ptr[kwp_data_index++];
                    sdo_data[7] = char_ptr[kwp_data_index++];
                    send_kwp_response();
                    kwp_data_length -= 6;
                    break; 
                }
            }
            break;
        case KWP_WR_DATA:  // write data
            kwp_object_id = Can_Rx_buf[dataIndex++];
            kwp_var_index = lookup_object_id(kwp_data_params, MAX_KWP_DATA_OBJECT_DEF, kwp_object_id);
            if (kwp_var_index < 0)
            {
                sdo_data[0] = 2;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = service_id;
                sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            }
            else
            {
                if((kwp_data_params[kwp_var_index].attr & READ_WRITE) != READ_WRITE)
                {
                    sdo_data[0] = 2;
                    sdo_data[1] = KWP_FAIL;
                    sdo_data[2] = service_id;
                    sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;
                }

                attr = kwp_data_params[kwp_var_index].attr;
                min_value = kwp_data_params[kwp_var_index].min;
                max_value = kwp_data_params[kwp_var_index].max;

                switch(attr & VAR_LENGTH_MASK) 
                {
                case FOUR_BYTES_LEN:
                    temp_data = sdo_data[3];
                    temp_data |= (((u32)sdo_data[4]) << 8);
                    temp_data |= (((u32)sdo_data[5]) << 16);
                    temp_data |= (((u32)sdo_data[6]) << 24);

                    if((((s32)temp_data < (s32)min_value || (s32)temp_data > (s32)max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == SIGNED_VAR)) || 
                        ((temp_data < min_value || temp_data > max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == 0)))
                    {
                        sdo_data[0] = 2;
                        sdo_data[1] = KWP_FAIL;
                        sdo_data[2] = service_id;
                        sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                    }
                    else
                    {
                        long_ptr = (u32 *) kwp_data_params[kwp_var_index].var;
                        *long_ptr = temp_data;
                        sdo_data[0] = 3;
                        sdo_data[1] = KWP_WR_DATA_OK;
                        sdo_data[2] = kwp_object_id;
                        sdo_data[3] = 0;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                        Save_Parameter(kwp_var_index);
                    }
                    send_kwp_response();
                    break;

                case TWO_BYTES_LEN:                          
                    temp_data = sdo_data[3];
                    temp_data |= (((u16)sdo_data[4]) << 8);
                    
                    if((((s16)temp_data < (s16)min_value || (s16)temp_data > (s16)max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == SIGNED_VAR)) || 
                        ((temp_data < min_value || temp_data > max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == 0)))    
                    {
                        sdo_data[0] = 2;
                        sdo_data[1] = KWP_FAIL;
                        sdo_data[2] = service_id;
                        sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                    }
                    else
                    {
                        short_ptr = (u16 *) kwp_data_params[kwp_var_index].var;
                        *short_ptr = temp_data;
                        sdo_data[0] = 3;
                        sdo_data[1] = KWP_WR_DATA_OK;
                        sdo_data[2] = kwp_object_id;
                        sdo_data[3] = 0;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                        Save_Parameter(kwp_var_index);
                    }
                    send_kwp_response();
                    break;
                case ONE_BYTES_LEN:
                    temp_data = sdo_data[3];
                    
                    if((((s8)temp_data < (s8)min_value || (s8)temp_data > (s8)max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == SIGNED_VAR)) || 
                        ((temp_data < min_value || temp_data > max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == 0)))         
                    {
                        sdo_data[0] = 2;
                        sdo_data[1] = KWP_FAIL;
                        sdo_data[2] = service_id;
                        sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                    }
                    else
                    {
                        char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                        *char_ptr = temp_data;
                        sdo_data[0] = 3;
                        sdo_data[1] = KWP_WR_DATA_OK;
                        sdo_data[2] = kwp_object_id;
                        sdo_data[3] = 0;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                        Save_Parameter(kwp_var_index);
                    }
                    send_kwp_response();
                    break;

                default:
                    data_length = kwp_rx_data_length - 2;
                    char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                    for(i = 0; i < data_length; i++)
                    {
                        char_ptr[i] = Can_Rx_buf[dataIndex++];
                    }

                    if(data_length < (attr & VAR_LENGTH_MASK))
                    {
                        char_ptr[data_length] = 0x00; // Add end character
                    }
                    Save_Parameter(kwp_var_index);
                    sdo_data[0] = 3;
                    sdo_data[1] = KWP_WR_DATA_OK;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = 0;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    
                    break; 
                }
            }
            break;
        case KWP_SECURITY:
            data_length = kwp_rx_data_length - 1;
            rv = secacc_process_request(TRUE, &Can_Rx_buf[dataIndex], &data_length);

            if(rv == 0)
            {
                sdo_data[0] = data_length + 1;
                sdo_data[1] = KWP_SECURITY_OK;
                
                for( i = 2; i < data_length + 2; i++)
                {
                    sdo_data[i] = Can_Rx_buf[i - 1];
                }
                
            }
            else
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = KWP_SECURITY;
                sdo_data[3] = rv;
                data_length = 2;
            }
            for(i = data_length + 2; i < 8; i++)
            {
                sdo_data[i] = 0;
            }
            send_kwp_response();
            break;

        case startRoutineByLocalIdentifier:
            kwp_object_id = Can_Rx_buf[dataIndex++];
            entry_option = Can_Rx_buf[dataIndex++];
            if((kwp_object_id == KWP_START_FLASH) && (entry_option == 0)) // start flash
            {
                sdo_data[0] = 2;
                sdo_data[1] = startRoutineByLocalIdentifier + 0x40;
                sdo_data[2] = kwp_object_id;
                sdo_data[3] = 0;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
                LCD_clear();
                Display_H24_Str("START", 2, 46, 0);
                Display_H24_Str("FLASHING SW", 3, 28, 0);
            }
            else if(kwp_object_id == KWP_DEL_FLASH) // delete flash
            {
                startAddr = entry_option;
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 24);
                flash_length = (((u32)Can_Rx_buf[dataIndex++]));
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 24);

                for(i = 0; i < 11; i++)
                {
                    part_number[i] = 0x30;
                }
                Save_Parameter(0);

                
                LCD_clear();
                
                Display_H24_Str((u8 *)"ERASING SW", 3, 32, 0);
                EraseFlash();  

                sdo_data[0] = 3;
                sdo_data[1] = startRoutineByLocalIdentifier + 0x40;
                sdo_data[2] = kwp_object_id;
                sdo_data[3] = 0x01;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();

                
            } 
            else if(kwp_object_id == KWP_CAL_CHECKSUM) // flash checksum
            {
                startAddr = entry_option;
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 24);
                
                endAddr = (((u32)Can_Rx_buf[dataIndex++]));
                endAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                endAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                endAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 24);
                
                checksum = (((u32)Can_Rx_buf[dataIndex++]));
                checksum |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                
               
                temp_checksum = 0;
                while(startAddr <= endAddr)
                {
                    temp_checksum += *(u8 *)startAddr;
                    startAddr++;
                }

                if(temp_checksum != checksum)
                {
                    sdo_data[0] = 3;
                    sdo_data[1] = KWP_FAIL;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = KWP_RESPONSE_CODE_INVALIDCHECKSUM;
                }
                else
                {
                    sdo_data[0] = 3;
                    sdo_data[1] = startRoutineByLocalIdentifier + 0x40;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = 0x00;
                }
    
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            } 
            else if(kwp_object_id == KWP_PASSIVE_MODE) // passive mod
            {
                if(entry_option == 0x00)
                {
                    sdo_data[0] = 2;
                    sdo_data[1] = startRoutineByLocalIdentifier + 0x40;
                    sdo_data[2] = 0x04;
                    sdo_data[3] = 0;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                }
            }
            else if(kwp_object_id == KWP_INIT01) // init01
            {
                if(entry_option == KWP_EMERGENCY_FLASH) 
                {
                    sdo_data[0] = 2;
                    sdo_data[1] = startRoutineByLocalIdentifier + 0x40;
                    sdo_data[2] = 0x80;
                    sdo_data[3] = 0;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_init01_response();
                    if(stay_for_ever == 0)
                    {
                        stay_for_ever = 1;
                        //load_parameters_from_ee();
                        if((FLASH_GetWriteProtectionOptionByte() &  0xffffff00) == 0xffffff00)// enable write
                        {
                            enableWRP(0);   
                        }
                        LCD_init();
                        test_pattern(0x00);
                    }
                }
            }
            
            break;
        case requestDownload:                                           // request download
            if ( secacc_get_state() != secacc_state_programming )
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = KWP_REQ_DOWNLOAD;
                sdo_data[3] = KWP_RESPONSE_CODE_ACCESSDENIED;   // can transfer 256 bytes
            }
            else
            {
                
                startAddr = (u32)Can_Rx_buf[dataIndex++];
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 24);

                dataIndex++; // skip invalid data;

                flash_length = (((u32)Can_Rx_buf[dataIndex++]));
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 24);

                sdo_data[0] = 3;
                sdo_data[1] = requestDownload + 0x40;
                sdo_data[2] = 0;
                sdo_data[3] = 1;   // can transfer 256 bytes
            }
            sdo_data[4] = 0;
            sdo_data[5] = 0;
            sdo_data[6] = 0;
            sdo_data[7] = 0;
            send_kwp_response();
            
            break;
        case transferData:                                             // transfer data
            if ( secacc_get_state() != secacc_state_programming )
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = KWP_REQ_DOWNLOAD;
                sdo_data[3] = KWP_RESPONSE_CODE_ACCESSDENIED;   // can transfer 256 bytes
                sdo_data[4] = 0;
                sdo_data[5] = 0;
            }
            else
            {
                if(flashing == 0)
                {
                    LCD_clear();
                }
                else if(flashing == FLASHING_DELAY_3)
                {
                    Display_H24_Str((u8 *)"FLASHING SW", 3, 32, 0);
                }
                else if(flashing == FLASHING_DELAY_4)
                {
                    flashing = -1;
                }
                flashing++;

                data_length = kwp_rx_data_length - 1;
                recevie_count += data_length;

                for(i = 1, k = 0; k < data_length/4; k++)
                {
                    temp_data = Can_Rx_buf[i++];
           
                    temp_data |= ((u32)Can_Rx_buf[i++]) << 8;
                    temp_data |= ((u32)Can_Rx_buf[i++]) << 16;
                    temp_data |= ((u32)Can_Rx_buf[i++]) << 24;
                    IWDG_ReloadCounter();
                    __set_PRIMASK(1);// disable interrupt
                    FLASH_Unlock();
                    
                    status = FLASH_ProgramWord(startAddr, temp_data);
                    FLASH_Lock();
                    __set_PRIMASK(0);// enable interrupt
                    IWDG_ReloadCounter();
                    startAddr += 4;
                    if(status != FLASH_COMPLETE)// exceptional handler
                    {
                        IWDG_ReloadCounter();
                        __set_PRIMASK(1);// disable interrupt
                        FLASH_Unlock();
                        FLASH_ProgramWord(ApplicationSwAddress, 0xAAAA);
                        FLASH_Lock();
                        __set_PRIMASK(0);// enable interrupt
                        IWDG_ReloadCounter();
                        while(1); // need to reset
                    }
                }                    
                IWDG_ReloadCounter();
                sdo_data[0] = 5;
                sdo_data[1] = transferData + 0x40;
                sdo_data[2] = recevie_count & 0xFF;
                sdo_data[3] = (recevie_count >> 8) & 0xFF;   // can transfer 256 bytes
                sdo_data[4] = (recevie_count >> 16) & 0xFF;   // can transfer 256 bytes
                sdo_data[5] = (recevie_count >> 24) & 0xFF;   // can transfer 256 bytes
            }    
            sdo_data[6] = 0;
            sdo_data[7] = 0;
            send_kwp_response();
            
            break;
        case requestTransferExit:                                       // exit transfer
            if ( secacc_get_state() != secacc_state_programming )
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = requestTransferExit;
                sdo_data[3] = KWP_RESPONSE_CODE_ACCESSDENIED;   // can transfer 256 bytes
            }
            else
            {
                startAddr = (u32)Can_Rx_buf[dataIndex++];
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 24);

                dataIndex++; // skip invalid data;

                flash_length = (((u32)Can_Rx_buf[dataIndex++]));
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 24);

                sdo_data[0] = 1;
                sdo_data[1] = requestTransferExit + 0x40;
                sdo_data[2] = 0;
                sdo_data[3] = 0;   // can transfer 256 bytes
            }
            sdo_data[4] = 0;
            sdo_data[5] = 0;
            sdo_data[6] = 0;
            sdo_data[7] = 0;
            send_kwp_response();
            break;

        case ecuReset:                                       // exit transfer
            kwp_object_id = Can_Rx_buf[dataIndex++];

            if(kwp_object_id == 0x01)
            {
                sdo_data[0] = 1;
                sdo_data[1] = ecuReset + 0x40;
                sdo_data[2] = 0;
                sdo_data[3] = 0;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();


                for( i = 0; i < 1000000; i++)
                {
                ;
                }
                NVIC_SystemReset();
            }
            break;
        case stopDiagnosticSession:                                       // exit transfer
            sdo_data[0] = 1;
            sdo_data[1] = stopDiagnosticSession + 0x40;
            sdo_data[2] = 0;
            sdo_data[3] = 0;
            sdo_data[4] = 0;
            sdo_data[5] = 0;
            sdo_data[6] = 0;
            sdo_data[7] = 0;
            send_kwp_response();
            break;
        case testerPresent:                                       // test presenter
            kwp_object_id = Can_Rx_buf[dataIndex++];
            if(kwp_object_id == 1)
            {
                sdo_data[0] = 1;
                sdo_data[1] = testerPresent + 0x40;
                sdo_data[2] = 0;
                sdo_data[3] = 0;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            }
            break; 
            
            
        default:
            break;
        }
    }
}
/**
* @brief	process CAN message
* @param	CanRxMsg* RxMessage: points to rx message
* @note     none
* @retval   none
*/
void process_msg(CanRxMsg* RxMessage)
{
    u32 id = RxMessage->StdId;
    u16 msgType = id & 0x7FF;

    if(RxMessage->IDE == CAN_Id_Extended) // Skip extended message
    {
        return;
    }
    
    switch(msgType) 
    {
    case NMT:
        break;

    case DISP_KWP_RX_ID:
    case DISP_KWP_RX_ID_ALL:
    case DISP_KWP_INIT01_RX_ID_ALL:
        process_kwp_mosi(RxMessage);
        break;

    default:
        break;
    }
    return;
}
/** Key Calculation for Security Access 1/2.
*
*  \param seed Input value.
*
*  \return Calculated Key.
*/
static u32 secacc_crypto_12( u32 seed )
{
   /* calculate key */
   const u32 a = 0x974c58ab;
   const u32 b = 0xfedcba98;
   const u32 c = 0x98765432;
   u32 x = seed + a;

   if( (0xffffffff - a) < seed)
   {
      x ^= b;
   }
   else
   {
      x ^= c;
   }

   x = (x >> 11) | ((x & 0x7ff) << 21);

   return x;
}

/** Key Calculation for Security Access 3/4.
*
*  \param seed Input value.
*
*  \return Calculated Key.
*/
static u32 secacc_crypto_34( u32 seed )
{
   return seed + 0x1AF7;
}

/** Key Calculation for Security Access 5/6.
*
*  \param seed Input value.
*
*  \return Calculated Key.
*/
static u32 secacc_crypto_56( u32 seed )
{
   u32 rv;
   u8 data[8];

   data[3] = (u8)seed;
   seed >>= 8;
   data[2] = (u8)seed;
   seed >>= 8;
   data[1] = (u8)seed;
   seed >>= 8;
   data[0] = (u8)seed;

   data[0] += 0x97;
   data[1] += 0x28;
   data[2] += 0xAA;
   data[3] += 0x55;

   rv = data[3];
   rv <<= 8;
   rv |= data[2];
   rv <<= 8;
   rv |= data[1];
   rv <<= 8;
   rv |= data[0];

   return rv;
}
/** Calculate Key
*
*  \param seed Input value.
*  \param state Kind of algorithm.
*
*  \return Calculated Key
*/
static u32 secacc_calculate_key( u32 seed, enum secacc_state state )
{
   switch(state)
   {
      case secacc_state_programming: return secacc_crypto_12( seed );
      case secacc_state_service    : return secacc_crypto_34( seed );
      case secacc_state_eopl       : return secacc_crypto_56( seed );
      default                      : return 0; /* will never happen */
   }
}
/** Copy 32Bit word into 4 Byte array.
*
*  \param dat Pointer to the destination.
*  \param value To by copied.
*/
static void secacc_u32_copy( u8 *dat, u32 value )
{
   dat[3] = (u8)(value >> 24);
   dat[2] = (u8)(value >> 16);
   dat[1] = (u8)(value >> 8);
   dat[0] = (u8)value;
}
static enum kwp_response_code secacc_seed( u8 condition, u8  *dat, u16 *len, enum secacc_state state)
{
   enum kwp_response_code rv;
   u32 seed;
   if (*len != 1) /* Wrong Length */
   {
      rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
   }
   else
   {
      if( FALSE == condition )
      {
         rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
      }
      else
      {
         if( state == secacc_security_state )
         {
            /* ECU already unlocked */
            dat[1] = 0;
            dat[2] = 0;
            *len   = 3;
            rv = KWP_RESPONSE_CODE_NONE;
         }
         else
         {
          seed = secacc_fortuna();
          secacc_code = secacc_calculate_key( seed, state );
          secacc_expected_service = dat[0]+1;
          *len = 5;
          secacc_u32_copy( &dat[1], seed );
          rv = KWP_RESPONSE_CODE_NONE;
         }
      }
   }
   return rv;
   
}


/**  Read 32 Bit word from given address.
*
* \param dat Source Pointer
*
* \return 32Bit value
*/
static u32 secacc_u32(u8 *dat )
{
   return ((u32)dat[3]<<24)+((u32)dat[2]<<16)+((u32)dat[1]<<8)+dat[0];
}



/** Process Key Request */
static enum kwp_response_code secacc_key( u8 *dat, u16 *len, enum secacc_state state)
{
   enum kwp_response_code rv;

   if (*len != 5)
   {  /* Invalid length */
      rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
   }
   else
   {
      if ( secacc_expected_service == dat[0])
      {
         /* Securitysecacc_state_access in progress */
         if( secacc_u32(&dat[1]) == secacc_code  )
         {  /* valid key */
            secacc_security_state = state;
            *len = 1;
            rv = KWP_RESPONSE_CODE_NONE;
         }
         else
         {  /* invalid key */
            rv = KWP_RESPONSE_CODE_INVALIDKEY;
         }
      }
      else
      {  /* request sequence error */
         rv = KWP_RESPONSE_CODE_REQUESTSEQUENCEERROR;
      }
   }
   return rv;
}
/**  Read 32 Bit word from given address.
*
* \param dat Source Pointer
*
* \return 32Bit value
*/

enum kwp_response_code secacc_process_request( u8 condition, u8  *dat, u16 *len)
{
   switch( dat[0] )
   {
#if SECACC_12
      case 0x01:  return secacc_seed( condition, dat, len, secacc_state_programming );
      case 0x02:  return secacc_key ( dat, len, secacc_state_programming );
#endif

#if SECACC_34
      case 0x03:  return secacc_seed( TRUE,  dat, len, secacc_state_service);
      case 0x04:  return secacc_key ( dat, len, secacc_state_service );
#endif

#if SECACC_56
      case 0x05:  return secacc_seed( TRUE, dat, len, secacc_state_eopl);
      case 0x06:  return secacc_key ( dat, len, secacc_state_eopl );
#endif
      default:    return KWP_RESPONSE_CODE_SUBFUNCTIONNOTSUPPORTED;
   }
}

enum secacc_state secacc_get_state( void )
{
   return secacc_security_state;
}



void secacc_set_state( enum secacc_state state )
{
   secacc_security_state = state;
}

void secacc_reset_state( void )
{
   secacc_security_state=secacc_state_default;
}

void Multi_msg_send(void)
{
    u8 i = 0;
    if(multi_send)
    {
        sdo_data[0] = 0x20 | (blkIndex%16);
        if(kwp_data_length >= 7)
        {

            for(i = 1;i < 8;i++)
            {
                sdo_data[i] = multi_char_ptr[kwp_data_index++];
            }
            kwp_data_length -= 7;
            if(kwp_data_length == 0)
            {
                multi_send = 0;
            }
        }
        else
        {
            for(i = 1;i < kwp_data_length+1;i++)
            {
                sdo_data[i] = multi_char_ptr[kwp_data_index++];
            }
            for(i = kwp_data_length+1;i < 8;i++)
            sdo_data[i] = 0;
            multi_send = 0;
            kwp_data_length = 0;
        }
        send_kwp_response();
        blkIndex++;
    }
}
u16 fortune_u16( struct fortune *fate )
{
   if( NULL == fate)
   {
      fate = &fortune_fate;
   }
   fate->random = fate->random * 1103515245 + 12345;
   return (u16)(fate->random/65536);
}


void fortune_set(struct fortune *fate, void *dest, u32 size )
{
   u8 *d=dest;
   while(size--)
   {
      *d++=(u8) fortune_u16(fate);
   }
}

u32 fortune_u32( struct fortune *fate )
{
    u32 rv;
    fortune_set(fate, &rv, sizeof(rv));
    return rv;
}

static void fortune_initialize(struct fortune *fate, u32 value)
{
   if( NULL == fate)
   {
      fate = &fortune_fate;
   }
   fate->random=value;
}


static u32 secacc_fortuna( void )
{
   struct fortune fortune_obj;
   fortune_initialize( &fortune_obj, osTick);
   return fortune_u32( &fortune_obj );
}


/**
* @brief	send sdo reponse
* @param	s16 type: reponse type
* @param	u32 result: reponse type
* @note     none
* @retval   s16
*/
static void send_kwp_init01_response(void)
{
    CanTxMsg txMsg = {0};

    txMsg.Data[0] = sdo_data[0];
    txMsg.Data[1] = sdo_data[1];
    txMsg.Data[2] = sdo_data[2];
    txMsg.Data[3] = sdo_data[3];
    txMsg.Data[4] = sdo_data[4];
    txMsg.Data[5] = sdo_data[5];
    txMsg.Data[6] = sdo_data[6];
    txMsg.Data[7] = sdo_data[7];

    txMsg.IDE = CAN_STD_ID;
    txMsg.StdId = DISP_KWP_INIT01_TX_ID_ALL; 
    txMsg.DLC = CAN_DATA_LEN_DEF;

    CAN_Transmit(CANOPEN_CAN, &txMsg);
    return;
}


